﻿using SoftFloat;
using System;
using System.Runtime.CompilerServices;

namespace BepuUtilities
{
    public struct Vector3
    {
        public sfloat X, Y, Z;

        public static Vector3 Zero
        {
            get
            {
                return default;
            }
        }
        public static Vector3 One
        {
            get
            {
                return new Vector3(1f);
            }
        }
        public static Vector3 UnitX
        {
            get
            {
                return new Vector3(1f, 0f, 0f);
            }
        }
        public static Vector3 UnitY
        {
            get
            {
                return new Vector3(0f, 1f, 0f);
            }
        }
        public static Vector3 UnitZ
        {
            get
            {
                return new Vector3(0f, 0f, 1f);
            }
        }

        public Vector3(sfloat value)
        {
            this = new Vector3(value, value, value);
        }

        public Vector3(sfloat x, sfloat y, sfloat z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        public Vector3(Vector2 value, sfloat z)
        {
            this = new Vector3(value.X, value.Y, z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static sfloat Dot(Vector3 vector1, Vector3 vector2)
        {
            return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Max(Vector3 value1, Vector3 value2)
        {
            return new Vector3((value1.X > value2.X) ? value1.X : value2.X, (value1.Y > value2.Y) ? value1.Y : value2.Y, (value1.Z > value2.Z) ? value1.Z : value2.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Min(Vector3 value1, Vector3 value2)
        {
            return new Vector3((value1.X < value2.X) ? value1.X : value2.X, (value1.Y < value2.Y) ? value1.Y : value2.Y, (value1.Z < value2.Z) ? value1.Z : value2.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
        {
            return new Vector3(vector1.Y * vector2.Z - vector1.Z * vector2.Y, vector1.Z * vector2.X - vector1.X * vector2.Z, vector1.X * vector2.Y - vector1.Y * vector2.X);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat LengthSquared()
        {
            return Dot(this, this);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat Length()
        {
            sfloat x = LengthSquared();
            return libm.Sqrt(x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Normalize(Vector3 value)
        {
            return value / value.Length();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Abs(Vector3 value)
        {
            return new Vector3(libm.Abs(value.X), libm.Abs(value.Y), libm.Abs(value.Z));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 SquareRoot(Vector3 value)
        {
            return new Vector3(libm.Sqrt(value.X), libm.Sqrt(value.Y), libm.Sqrt(value.Z));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float DistanceSquared(Vector3 value1, Vector3 value2)
        {
            Vector3 vector = value1 - value2;
            return Dot(vector, vector);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static sfloat Distance(Vector3 value1, Vector3 value2)
        {
            sfloat x = DistanceSquared(value1, value2);
            return libm.Sqrt(x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Transform(Vector3 value, Quaternion rotation)
        {
            sfloat num = rotation.X + rotation.X;
            sfloat num2 = rotation.Y + rotation.Y;
            sfloat num3 = rotation.Z + rotation.Z;
            sfloat num4 = rotation.W * num;
            sfloat num5 = rotation.W * num2;
            sfloat num6 = rotation.W * num3;
            sfloat num7 = rotation.X * num;
            sfloat num8 = rotation.X * num2;
            sfloat num9 = rotation.X * num3;
            sfloat num10 = rotation.Y * num2;
            sfloat num11 = rotation.Y * num3;
            sfloat num12 = rotation.Z * num3;
            return new Vector3(value.X * (1f - num10 - num12) + value.Y * (num8 - num6) + value.Z * (num9 + num5), value.X * (num8 + num6) + value.Y * (1f - num7 - num12) + value.Z * (num11 - num4), value.X * (num9 - num5) + value.Y * (num11 + num4) + value.Z * (1f - num7 - num10));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator +(Vector3 left, Vector3 right)
        {
            return new Vector3(left.X + right.X, left.Y + right.Y, left.Z + right.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator -(Vector3 left, Vector3 right)
        {
            return new Vector3(left.X - right.X, left.Y - right.Y, left.Z - right.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator *(Vector3 left, sfloat right)
        {
            return left * new Vector3(right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator *(Vector3 left, Vector3 right)
        {
            return new Vector3(left.X * right.X, left.Y * right.Y, left.Z * right.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator /(Vector3 left, Vector3 right)
        {
            return new Vector3(left.X / right.X, left.Y / right.Y, left.Z / right.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator /(Vector3 value1, sfloat value2)
        {
            return value1 / new Vector3(value2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator -(Vector3 value)
        {
            return Zero - value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 operator *(sfloat left, Vector3 right)
        {
            return right * left;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(Vector3 left, Vector3 right)
        {
            return left.X == right.X && left.Y == right.Y && left.Z == right.Z;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(Vector3 left, Vector3 right)
        {
            return !(left == right);
        }

        public override string ToString()
        {
            return $"({X.ToString("G")}, {Y.ToString("G")}, {Z.ToString("G")})";
        }
    }
}